#include "kernel.h"

unsigned int kernel_sp, last_proc_sp; 


char current_proc;

ISR(TIMER0_OVF_vect)  //Scheduler
{
	saveState();

	last_proc_sp = SP;
	if(current_proc == 0) kernel_sp = SP;
	SP = kernel_sp;
	process[current_proc].stack_pointer = last_proc_sp;
	//Maybe disable interrupts here?
	TCCR0B &= 0xF8;
	TCNT0 = 0;
	char i, lo, hi;
		for(i = 0; i < NUM_PROCS + 1; i++)
		{
			current_proc++;
			if(current_proc > NUM_PROCS) current_proc = 0;
			if(process[current_proc].state == RUNNING) break;
			if(process[current_proc].state == QUEUED)
			{
				process[current_proc].state = RUNNING;
				asm("STS 0x0176,R18");
				SP = process[current_proc].stack_pointer;
				TCCR0B |= 0x02;
				lo = program[process[current_proc].program].offset_address & 0x00FF;
				hi = program[process[current_proc].program].offset_address >> 8;
				
				asm volatile("mov r30, %0\n\t"
							 "mov r31, %1\n\t"
							 "ijmp\n\t"
							 :
							 : "r" (lo),"r" (hi));
			}	
		}

	SP = process[current_proc].stack_pointer;
	TCCR0B |= 0x02;
	returnState();
}


inline void returnState(void)
{
	asm("pop r2\n\t"
		"pop r3\n\t"
		"pop r4\n\t"
		"pop r5\n\t"
		"pop r6\n\t"
		"pop r7\n\t"
		"pop r8\n\t"
		"pop r9\n\t"
		"pop r10\n\t"
		"pop r11\n\t"
		"pop r12\n\t"
		"pop r13\n\t"
		"pop r14\n\t"
		"pop r15\n\t"
		"pop r16\n\t"
		"pop r17\n\t"
		"pop r20\n\t"
		"pop r21\n\t"
		"pop r22\n\t"
		"pop r23\n\t"
		"pop r26\n\t"
		"pop r27\n\t"
		"pop r28\n\t"
		"pop r29\n\t");
}

inline void saveState(void)
{
	asm("push r29\n\t"
		"push r28\n\t"
		"push r27\n\t"
		"push r26\n\t"
		"push r23\n\t"
		"push r22\n\t"
		"push r21\n\t"
		"push r20\n\t"
		"push r17\n\t"
		"push r16\n\t"
		"push r15\n\t"
		"push r14\n\t"
		"push r13\n\t"
		"push r12\n\t"
		"push r11\n\t"
		"push r10\n\t"
		"push r9\n\t"
		"push r8\n\t"
		"push r7\n\t"
		"push r6\n\t"
		"push r5\n\t"
		"push r4\n\t"
		"push r3\n\t"
		"push r2\n\t");
}
